//
// Created by luo on 2022-10-16.
//

#include "../include/AngleSolve.h"
#include "opencv2/opencv.hpp"


/*
 * 姿态解算
 * 1、已知多对3D-2D匹配点对的世界坐标系坐标以及像素坐标系坐标(装甲板的四点<->灯条的四点)
      1-1 相机中心偏离目标中心时，需要转动的角度Q与云台转动的yaw和pitch角度向量的关系
      1-2 摄像头安装的水平位置与云台轴承的水平位置差，算出的摄像头转换角度仍需转换到转轴实际转动角度
          要算出云台实际转动角度，除了yaw和pitch值还需要直到摄像头距离目标的长度l，再加上摄像头位置到
          轴承位置的水平距离d，像素中目标点偏移中心距离D/云台轴承到中心点距离l+d 得到正切值，再反正切求出实际转动角度

 * 2、已知相机内参--已知映射过程
        多个重要点的坐标(实物)->映射->多个重要点坐标(像素)
 * 3、利用PNP解算还原丢失掉的纵深信息(P3P等)
      3-1 单目测距,利用小孔成像原理，相似三角形原理,由于（镜头焦距与装甲板实际大小不变）
          由灯条的像素长度/间距像素长度计算出实际距离，纵向选后者，因为前者会由于倾斜损失长度
          横向选前者，因为后者会增加额外长度
      3-2 当装甲板横向和纵向都有倾斜时，则3-1计算都不准确，用到PNP算法
          3-2-1 张定友标定，对摄像头进行标定，得出相机内参，求出内参矩阵和畸变矩阵，实现图像去畸变
          3-2-2 PNP解算，SolvePnP()
                返回的平移矩阵transfer_matrix(x,y,z)，利用距离公式可求出距离值dis
                Rotation_matrix(9个参数)，利用公式变换由反三角函数求出roll,yaw,pitch
          3-2-3 最终得出真实的云台转动角度


 * 4、所有信息代入抽象模型，反解得出相对姿态，即旋转矩阵和平移矩阵
        旋转矩阵->姿态角(roll,yaw,pitch) 平移矩阵->对应x,y,z的偏量
 * */

bool AngleSolve::AngleSolve_demo(Armor_detection &armor,std::vector<int> serial_scale,std::vector<int> &output_scale,double &absoluteDistance)
{
    //turn_scale.clear();
    if(armor.Armor::points.size()<4)
        return false;

    PnP_algorithm(armor,"small",armor.Armor::points);
    Angle_solve(armor,15,output_scale[1],absoluteDistance);
    output_scale = angle_set(serial_scale);

    return true;
}

//1、PNP测距求出平移矩阵和旋转矩阵
bool AngleSolve::PnP_algorithm(Armor_detection &armor,std::string deck_type,std::vector<cv::Point2f> deck_2D)
{
    float half_len,half_wid;
    if(deck_type == "small") //小装甲板长宽一半
    {
        half_len = 69;
        half_wid = 28;
    }
    else if(deck_type == "big")
    {
        half_len = 116.5;
        half_wid = 28;
    }
    else
        return false;

    std::vector<Point3f> deck_3D = std::vector<Point3f>{
        cv::Point3f (-half_len,half_wid,0),
        cv::Point3f (-half_len,-half_wid,0),
        cv::Point3f (half_len,-half_wid,0),
        cv::Point3f (half_len,half_wid,0)
    };

    double x,y,z,theta_yaw,theta_pitch;

    //导入标定文件，输出内参矩阵和畸变矩阵
//    FileStorage os("../camera_data.xml",FileStorage::READ);//读取摄像头标定文件
//    os["internal_param_matrix"] >> this->internal_param_matrix;
//    os["distortion_matrix"] >> this->distortion_matrix;

    //直接利用条件编译选取相应摄像头的内参

#if USB
    //camera_matrix = cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(0));
    // camera_matrix.ptr<double>(0)[0] = 1567.999;
    // camera_matrix.ptr<double>(0)[2] = 320;
    // camera_matrix.ptr<double>(1)[1] = 1456.789;
    // camera_matrix.ptr<double>(1)[2] = 240;
    // camera_matrix.ptr<double>(2)[2] = 1.0f;
	camera_matrix.ptr<double>(0)[0] = 1280.999;
    camera_matrix.ptr<double>(0)[2] = 320;
    camera_matrix.ptr<double>(1)[1] = 1270.789;
    camera_matrix.ptr<double>(1)[2] = 240;
    camera_matrix.ptr<double>(2)[2] = 1.0f;


    //相机的畸变参数矩阵
    distortion_matrix = cv::Mat(5, 1, CV_64FC1, cv::Scalar::all(0));
    distortion_matrix.ptr<double>(0)[0] = -0.001;
    distortion_matrix.ptr<double>(1)[0] = 0.001;
    distortion_matrix.ptr<double>(2)[0] = -0.001;
    distortion_matrix.ptr<double>(3)[0] = 0.001;
    distortion_matrix.ptr<double>(4)[0] = 0;
#endif
#if MV0

    // camera_matrix.ptr<double>(0)[0] = 960.714678;//540.6846;//3227.714678; 	//fx
    // camera_matrix.ptr<double>(0)[2] = 320.943891;		//cx  320
    // camera_matrix.ptr<double>(1)[1] = 960.714678;	    //fy
    // camera_matrix.ptr<double>(1)[2] = 240.057302;		//cy 240
    // camera_matrix.ptr<double>(2)[2] = 1.0f;


    // //相机的畸变参数矩阵 k1，k2，p1，p2，k3

    // distortion_matrix.ptr<double>(0)[0] = -1.031940;//-0.294792;//-1.031940
    // distortion_matrix.ptr<double>(1)[0] =-1.793571;//0.152200;//-1.793571
    // distortion_matrix.ptr<double>(2)[0] = 0.000;
    // distortion_matrix.ptr<double>(3)[0] = 0.000000;
    // distortion_matrix.ptr<double>(4)[0] = 1,927041;//-0.086483; //1,927041
	camera_matrix.ptr<double>(0)[0] = 1230.714678;//540.6846;//3227.714678; 	//fx
    camera_matrix.ptr<double>(0)[2] = 320.943891;		//cx  320
    camera_matrix.ptr<double>(1)[1] = 1230.714678;	    //fy
    camera_matrix.ptr<double>(1)[2] = 240.057302;		//cy 240
    camera_matrix.ptr<double>(2)[2] = 1.0f;


    //相机的畸变参数矩阵 k1，k2，p1，p2，k3

    distortion_matrix.ptr<double>(0)[0] = -0.478789;//-0.294792;//-1.031940
    distortion_matrix.ptr<double>(1)[0] =1.980264;//0.152200;//-1.793571
    distortion_matrix.ptr<double>(2)[0] = 0.000;
    distortion_matrix.ptr<double>(3)[0] = 0.000000;
    distortion_matrix.ptr<double>(4)[0] = -5,768084;//-0.086483; //1,927041
#endif

#if MV1

    camera_matrix.ptr<double>(0)[0] = 1259.706299; 	//fx
    camera_matrix.ptr<double>(0)[2] = 320;			//cx
    camera_matrix.ptr<double>(1)[1] = 1259.706299;	//fy
    camera_matrix.ptr<double>(1)[2] = 240;			//cy
    camera_matrix.ptr<double>(2)[2] = 1.0f;


    //相机的畸变参数矩阵 k1，k2，p1，p2，k3

    distortion_matrix.ptr<double>(0)[0] = 0.01915;
    distortion_matrix.ptr<double>(1)[0] = 0.079099;
    distortion_matrix.ptr<double>(2)[0] = -0.000000;
    distortion_matrix.ptr<double>(3)[0] = 0.000000;
    distortion_matrix.ptr<double>(4)[0] = -0.941516;
#endif
#if MV2

    camera_matrix.ptr<double>(0)[0] = 1578.8623;//975.130030;//1110,268716; 	//fx
    camera_matrix.ptr<double>(0)[2] = 294;			//cx
    camera_matrix.ptr<double>(1)[1] = 1578.8623;//975.130030;//1110,268716;	//fy
    camera_matrix.ptr<double>(1)[2] = 315;			//cy
    camera_matrix.ptr<double>(2)[2] = 1.0f;


    //相机的畸变参数矩阵 k1，k2，p1，p2，k3

    // distortion_matrix.ptr<double>(0)[0] = -0.459325;
    // distortion_matrix.ptr<double>(1)[0] = 2.636423;
    // distortion_matrix.ptr<double>(2)[0] = -0.000000;
    // distortion_matrix.ptr<double>(3)[0] = 0.000000;
    // distortion_matrix.ptr<double>(4)[0] = -11.502936;

    distortion_matrix.ptr<double>(0)[0] = -0.4064;
    distortion_matrix.ptr<double>(1)[0] = 0.9349;
    distortion_matrix.ptr<double>(2)[0] = -0.000000;
    distortion_matrix.ptr<double>(3)[0] = 0.000000;
    distortion_matrix.ptr<double>(4)[0] = 0.000;
	//camera_matrix = cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(0));
	// camera_matrix.ptr<double>(0)[0] = 600.244691; 	//fx
	// camera_matrix.ptr<double>(0)[2] = 320;			//cx
	// camera_matrix.ptr<double>(1)[1] = 600,244691;	//fy
	// camera_matrix.ptr<double>(1)[2] = 240;			//cy
	// camera_matrix.ptr<double>(2)[2] = 1.0f;


	// //相机的畸变参数矩阵 k1，k2，p1，p2，k3
	// distortion_matrix = cv::Mat(5, 1, CV_64FC1, cv::Scalar::all(0));
	// distortion_matrix.ptr<double>(0)[0] = -0.292023;
	// distortion_matrix.ptr<double>(1)[0] = 0.133750;
	// distortion_matrix.ptr<double>(2)[0] = -0.000000;
	// distortion_matrix.ptr<double>(3)[0] = 0.000000;
	// distortion_matrix.ptr<double>(4)[0] = -0.057357;
#endif

#if MV3
    camera_matrix.ptr<double>(0)[0] = 1730.706299;//1659.706299; 	//fx 1280x1080
    camera_matrix.ptr<double>(0)[2] = 400;//320;			//cx
    camera_matrix.ptr<double>(1)[1] = 1730.706299;	//fy
    camera_matrix.ptr<double>(1)[2] = 400;//240;			//cy
    camera_matrix.ptr<double>(2)[2] = 1.0f;


    //相机的畸变参数矩阵 k1，k2，p1，p2，k3

    distortion_matrix.ptr<double>(0)[0] = 0.01915;
    distortion_matrix.ptr<double>(1)[0] = 0.079099;
    distortion_matrix.ptr<double>(2)[0] = -0.000000;
    distortion_matrix.ptr<double>(3)[0] = 0.000000;
    distortion_matrix.ptr<double>(4)[0] = -0.941516;
#endif

    //PnP解算函数,得出平移矩阵和旋转矩阵
    solvePnP(deck_3D,deck_2D,this->camera_matrix,this->distortion_matrix,
             this->Rotation_matrix,this->transfer_matrix, false,SOLVEPNP_ITERATIVE);

    std::cout << "transfer_matrix" << this->transfer_matrix << std::endl;
    /*
        x = transfer_matrix.at<double>(1, 1);
        y = transfer_matrix.at<double>(2, 1);
        z = transfer_matrix.at<double>(3, 1);
        theta_yaw = atan2(x, z);
        theta_pitch = atan2(y, z);
    */

}

//2、算出云台转动相对刻度
bool AngleSolve::Angle_solve(Armor_detection &best_armor,int speed,int gunpitch_rightnow,double & absolute_Distance) {

    int turn_yaw,turn_pitch;
    double x,y,z,theta_yaw,theta_pitch,theta_pitch_rightnow;
    if(transfer_matrix.empty()) return false;

    //偏移矩阵的x，y，z偏移量 + 云台轴承距离摄像头光心的偏移量
    x = transfer_matrix.at<double>(0, 0)+X;
    y = -transfer_matrix.at<double>(1, 0)-Y;
    z = transfer_matrix.at<double>(2, 0)+Z;
    best_armor.Armor::tx=x;
    best_armor.Armor::ty=y;
    best_armor.Armor::tz=z;

    absolute_Distance = sqrt(x * x + y * y + z * z);//云台轴承处距离目标装甲板中心的绝对距离

    //PnP解算得出的x，y，z反推角度yaw和pitch
    theta_yaw = atan2(x , z) * 180 / PI;
    theta_pitch = atan2(y, z) * 180 / PI;

    theta_pitch_rightnow = gunpitch_rightnow - PitchAngleofHorizen; //PitchAngleofHorizen为pitch的最大刻度?
    float gunPitchAngle_offset_horizen = -(theta_pitch_rightnow / PITCH * 360);
    float decent_Angle = (theta_pitch + gunPitchAngle_offset_horizen) *  PI / 180.0f;

    //在没有外力的情况下，子弹射出时默认是在Y-O-Z的竖直平面上做平抛运动，顾不需考虑X轴位移
//    float relative_Distance = absolute_Distance * cos(decent_Angle);
//    float height = absolute_Distance * sin(decent_Angle);

    //经过平抛运动模型运算后，算出的实际抬升角度
    theta_pitch = solve_AngleWithGravity(theta_pitch, speed, absolute_Distance/1000, gunPitchAngle_offset_horizen);

    //减缓角度转动速度防止抖动，需要实际测出
    if (abs(theta_yaw) > 10) theta_yaw *= 0.9;
    if (abs(theta_pitch) > 10) theta_pitch *= 0.9;

    //解算得出云台转动刻度
    turn_yaw = theta_yaw * YAW / 360;
    turn_pitch = theta_pitch * PITCH / 360;

    //相机角度补偿,修正摄像头和枪管的角度差
    turn_yaw +=27;
    //枪管抬升补偿
    turn_pitch+=+95;//green_bullet

    if(absolute_Distance>=4200)
        turn_pitch+=+70;
    this->turn_angle.push_back(turn_yaw);
    this->turn_angle.push_back(turn_pitch);
}

//3、重力补偿算法
double AngleSolve::solve_AngleWithGravity(float camera_SolveAngle,float speed,float absolute_Distance,float gunPitch_AngleoffsetHorizen)
{
    float decent_Angle = (camera_SolveAngle + gunPitch_AngleoffsetHorizen) * PI / 180.0f;

    //在没有外力的情况下，子弹射出时默认是在Y-O-Z的竖直平面上做平抛运动，顾不需考虑X轴位移
    float relative_Distance = absolute_Distance * cos(decent_Angle);
    float height = absolute_Distance * sin(decent_Angle);

    //使用斜抛运动方程，二元一次方程组求解子弹飞行曲线
    float a,b,c,delta,mid; //二元一次方程的系数abc，的他 与两根之和
    c = ( (a = -G * relative_Distance * relative_Distance / (2 * speed * speed)), (b = relative_Distance), (-height + a) );
    delta = b*b - 4*a*c;
    mid = -b / (2*a);

    float angle,x1 = 0.0f,x2 = 0.0f;
    if(delta<0){
        angle = decent_Angle;
    }
    else if(delta == 0){
        x1 = mid;
        angle = atanf(x1) * 180 / PI;
    }
    else{
        //求根公式
        x1 = mid - sqrtf(delta) / (2 * a);
        x2 = mid + sqrtf(delta) / (2 * a);
        if(x1>= -1 && x1<=1){
            angle = atanf(x1) * 180 / PI;
        }
        else if(x2>= -1 && x2<=1){
            angle = atanf(x2) * 180 / PI;
        }
    }

    return angle - gunPitch_AngleoffsetHorizen;
}

//4、算出云台转动的期望刻度
std::vector<int> AngleSolve::angle_set(std::vector<int> &scale)
{
    int yaw, pitch;
    if(turn_angle.size()<2){
        this->turn_angle.push_back(0);
        this->turn_angle.push_back(0);
    }
    cout<<"turn_pitch:"<<turn_angle[1]<<endl;

    yaw = -this->turn_angle[0] + scale[0];
    pitch = -this->turn_angle[1] + scale[1];
    //防止出现刻度为负以及大于最大刻度的情况
    yaw += YAW;
    pitch += PITCH;
    yaw %= YAW;
    pitch %= PITCH;

    scale[0] = yaw;
    scale[1] = pitch;
    //this->scale=scale;587
    return scale;
}



/*
//算出云台转动相对刻度
std::vector<int> AngleSolve::Cradlehead_turn(cv::Mat transfer_matrix,int speed)
{
    int Cradlehead_turn_yaw,Cradlehead_turn_pitch;
    double t_x,t_y,t_z,theta_yaw,theta_pitch;
    std::vector<int>turn_scale;
    t_x = transfer_matrix.at<double>(0,0);
    t_y = transfer_matrix.at<double>(1,0);
    t_z = transfer_matrix.at<double>(2,0);

    //PnP解算得出的x，y，z反推角度yaw和pitch
    theta_yaw = atan2(t_x+X,t_z+Z) * 180 / PI; // yaw
    theta_pitch = atan2(t_y+Y,t_z+Z) * 180 / PI; // pitch

    //减缓角度转动速度防止抖动，需要实际测出
    if(abs(theta_yaw)>10) theta_yaw*=0.8;
    if(abs(theta_pitch)>10) theta_pitch*=0.9;

    //PnP解算得出云台转动刻度
    Cradlehead_turn_yaw = theta_yaw * YAW / 360;
    Cradlehead_turn_pitch = theta_pitch * PITCH / 360;

    //相机角度补偿,修正摄像头和枪管的角度差
    Cradlehead_turn_yaw += 10;
    Cradlehead_turn_pitch += 5;

    //枪管抬升补偿
    Cradlehead_turn_pitch += 5;

    turn_scale.push_back(Cradlehead_turn_yaw);
    turn_scale.push_back(Cradlehead_turn_pitch);
    return turn_scale;
}

//算出云台转动的期望刻度
std::vector<int> AngleSolve::Cradlehead_set(std::vector<int> yuntai_wish_scale,std::vector<int> turn_scale)
{
    int yaw,pitch;
    yaw = turn_scale[0] + yuntai_wish_scale[0];
    pitch = turn_scale[1]+yuntai_wish_scale[1];

    //防止出现刻度为负以及大于最大刻度的情况
    yaw += YAW;
    pitch += PITCH;
    yaw %= YAW;
    pitch %= PITCH;

    yuntai_wish_scale[0] = yaw;
    yuntai_wish_scale[1] = pitch;

    return yuntai_wish_scale;
}
 */